#include "calculatethreepointmatrixtransform.h"
#include <kdl/frames.hpp>

//https://www.jianshu.com/p/58cf5655f9a9
//https://blog.csdn.net/weixin_43200933/article/details/95388014
//https://blog.csdn.net/qq_17386217/article/details/106536549?utm_medium=distribute.pc_relevant_download.none-task-blog-blogcommendfrombaidu-3.nonecase&depth_1-utm_source=distribute.pc_relevant_download.none-task-blog-blogcommendfrombaidu-3.nonecas
CalculateThreePointMatrixTransform::CalculateThreePointMatrixTransform()
{

}


int CalculateThreePointMatrixTransform::calculateTransform(vector<float> point1onA,vector<float> point2onA,vector<float> point3onA,
                                                           vector<float> point1onB,vector<float> point2onB,vector<float> point3onB
                                ,Eigen::Matrix3f& rotationOut, Eigen::Vector3f& translationOut,float &ratioOut)
{
    caliratio( point1onA, point2onA, point3onA,
                             point1onB, point2onB, point3onB  ,ratioOut);

    if(1!=caliRotation(point1onA, point2onA, point3onA,
                 point1onB, point2onB, point3onB  ,ratioOut, rotationOut))
    {
        return -1;
    }
    caliTranslation(point1onA, point1onB,rotationOut,  translationOut);
    return 1;

}

int CalculateThreePointMatrixTransform::calculateTransformNew(vector<double> point1onA,
                            vector<double> point2onA, vector<double> point3onA,
                 vector<double> point1onB, vector<double> point2onB, vector<double> point3onB,
               Eigen::Matrix3f &rotationOut, Eigen::Vector3f &translationOut, float &ratioOut)
{
//    Point1InA= [-6.1896;6.2744;-2.6239;1];
//    Point2InA= [21.1387;-11.7097; -2.7459;1];
//    Point3InA= [11.1252;10.1226;-2.4942;1];

    //    Point1InB =[36.412;0.0578;1.8355;1];
    //    Point2InB= [-2.1563;4.9717;-1.664;1];
    //    Point3InB =[35.8232;-0.364;0.893;1];
    Eigen::Vector3f Point1InA ;
    Point1InA<<point1onA[0],point1onA[1],point1onA[2];
    Eigen::Vector3f Point2InA ;
    Point2InA<<point2onA[0],point2onA[1],point2onA[2];
    Eigen::Vector3f Point3InA ;
    Point3InA<<point3onA[0],point3onA[1],point3onA[2];



    Eigen::Vector3f Point1InB ;
    Point1InB<<point1onB[0],point1onB[1],point1onB[2];
    Eigen::Vector3f Point2InB ;
    Point2InB<<point2onB[0],point2onB[1],point2onB[2];
    Eigen::Vector3f Point3InB ;
    Point3InB<<point3onB[0],point3onB[1],point3onB[2];

//    vectorB12=Point1InB(1:3,1)-Point2InB(1:3,1);
//    vectorB13=Point1InB(1:3,1)-Point3InB(1:3,1);
//    vectorB32=cross(vectorB12,vectorB13);
//    vectorB13=cross(vectorB32,vectorB12);
     Eigen::Vector3f  vectorB12=Point1InB-Point2InB;
     Eigen::Vector3f  vectorB13=Point1InB-Point3InB;
     Eigen::Vector3f  vectorB32=vectorB12.cross(vectorB13);
     vectorB13=vectorB32.cross(vectorB12);

//    vectorA12=Point1InA(1:3,1)-Point2InA(1:3,1);
//    vectorA13=Point1InA(1:3,1)-Point3InA(1:3,1);
//    vectorA32=cross(vectorA12,vectorA13);
//    vectorA13=cross(vectorA32,vectorA12);
     Eigen::Vector3f vectorA12=Point1InA-Point2InA;
     Eigen::Vector3f vectorA13=Point1InA-Point3InA;
     Eigen::Vector3f vectorA32=vectorA12.cross(vectorA13);
     vectorA13=vectorA32.cross(vectorA12);

//    ArrayB=[vectorB12,vectorB13,vectorB32];
//    ArrayA=[vectorA12,vectorA13,vectorA32];

     Eigen::Matrix3f ArrayB;
     ArrayB<<vectorB12,vectorB13,vectorB32;
     Eigen::Matrix3f ArrayA;
     ArrayA<<vectorA12,vectorA13,vectorA32;
     Eigen::MatrixXf ready;
     ready<<ArrayB.transpose(),ArrayA.transpose();
//    ready=[ArrayB',ArrayA'];
//    RotateT=rref(ready);
//    Rotate=RotateT(1:3,4:6)';
//    move=Point1InA(1:3,1)-Rotate*Point1InB(1:3,1);
//    result(1:3,1:3)=Rotate;
//    result(1:3,4)=move;
//    result(4,1:3)=0;
     //    result(4,4)=1;
}

int CalculateThreePointMatrixTransform::calculateTransformNew2(vector<double> point1onA, vector<double> point2onA,
                                                               vector<double> point3onA, vector<double> point1onB,
                                                               vector<double> point2onB, vector<double> point3onB,
                               Eigen::Matrix3f &rotationOut, Eigen::Vector3f &translationOut, float &ratioOut)
{
    double Point1InA[3];
    Point1InA[0]=point1onA[0];
    Point1InA[1]=point1onA[1];
    Point1InA[2]=point1onA[2];
    double Point2InA[3];
    Point2InA[0]=point2onA[0];
    Point2InA[1]=point2onA[1];
    Point2InA[2]=point2onA[2];
    double Point3InA[3];
    Point3InA[0]=point3onA[0];
    Point3InA[1]=point3onA[1];
    Point3InA[2]=point3onA[2];
    double Point1InB[3];
    Point1InB[0]=point1onB[0];
    Point1InB[1]=point1onB[1];
    Point1InB[2]=point1onB[2];
    double Point2InB[3];
    Point2InB[0]=point2onB[0];
    Point2InB[1]=point2onB[1];
    Point2InB[2]=point2onB[2];
    double Point3InB[3];
    Point3InB[0]=point3onB[0];
    Point3InB[1]=point3onB[1];
    Point3InB[2]=point3onB[2];
    double dOutRotateMatrixAToB[9];
    double dOutTranslationMatrixAToB[3];
    double Deviation[9];
    ThreePointsTransMatrix(Point1InA,Point2InA,Point3InA,Point1InB,Point2InB,Point3InB,
                           dOutRotateMatrixAToB,dOutTranslationMatrixAToB,Deviation);


    rotationOut<<dOutRotateMatrixAToB[0],dOutRotateMatrixAToB[1],dOutRotateMatrixAToB[2],
            dOutRotateMatrixAToB[3],dOutRotateMatrixAToB[4],dOutRotateMatrixAToB[5],
            dOutRotateMatrixAToB[6],dOutRotateMatrixAToB[7],dOutRotateMatrixAToB[8];

    KDL::Rotation tmpRotation(rotationOut(0),rotationOut(1),rotationOut(2),
            rotationOut(3),rotationOut(4),rotationOut(5),
            rotationOut(6),rotationOut(7),rotationOut(8));
    //需要去掉比例系数
    double tmpW,tmpP,tmpR;
    tmpRotation.GetRPY(tmpW,tmpP,tmpR);
    KDL::Rotation newRotate;
//    tmpRotation.Inverse();
//    rotationOut<<tmpRotation.data[0],tmpRotation.data[1],tmpRotation.data[2],
//            tmpRotation.data[3],tmpRotation.data[4],tmpRotation.data[5],
//            tmpRotation.data[6],tmpRotation.data[7],tmpRotation.data[8];

    //

    KDL::Frame tmpFrame = KDL::Frame(newRotate.RPY(tmpW,tmpP,tmpR).Inverse(),
                    KDL::Vector (dOutTranslationMatrixAToB[0],
                        dOutTranslationMatrixAToB[1],
                        dOutTranslationMatrixAToB[2]));
    KDL::Frame tmpFrameNew=tmpFrame.Inverse();
    translationOut<<tmpFrameNew.p[0],tmpFrameNew.p[1],tmpFrameNew.p[2];



    return 1;

}

int CalculateThreePointMatrixTransform::calculateTransform2(vector<vector<float> > point1onA,
                vector<vector<float> > point1onB, Eigen::Matrix3f &rotationOut,
                                     Eigen::Vector3f &translationOut, float &ratioOut)
{
    Eigen::MatrixXf centroid_A(3,1) ;
    centroid_A<<0,0,0;
    Eigen::MatrixXf centroid_B(3,1) ;
    centroid_B<<0,0,0;

    for(int i=0;i<point1onA.size();i++)
    {
        Eigen::MatrixXf tmp1(3,1) ;
        tmp1<<point1onA[i][0],point1onA[i][1],point1onA[i][2];
        Eigen::MatrixXf tmp2(3,1) ;
        tmp2<<point1onB[i][0],point1onB[i][1],point1onB[i][2];
        centroid_A=centroid_A+tmp1;
        centroid_B=centroid_B+tmp2;
    }
    centroid_A=centroid_A/point1onA.size();
    centroid_B=centroid_B/point1onA.size();

    Eigen::MatrixXf H;
    for(int i=0;i<point1onA.size();i++)
    {
        Eigen::MatrixXf tmpJ;
        Eigen::MatrixXf tmpP1A(3,1);
        Eigen::MatrixXf tmpP1B(3,1);
        Eigen::MatrixXf tmpBT=tmpP1B-centroid_B;
        tmpJ=(tmpP1A-centroid_A)*(tmpBT.transpose());
        if(0==i)
        {
            H=tmpJ;
        }
        else
        {
            H=H+tmpJ;
        }
    }


    Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::MatrixXf   U = svd.matrixU();
    Eigen::MatrixXf   V = svd.matrixV();
    rotationOut=V*(U.transpose());//精度有问题．

    translationOut=-rotationOut*centroid_A+centroid_B;
    return 1;


}

int CalculateThreePointMatrixTransform:: caliratio(vector<float> point1onA,vector<float> point2onA,vector<float> point3onA,
                        vector<float> point1onB,vector<float> point2onB,vector<float> point3onB  ,float &ratioOut)
{
        vector<float> ratiotemp;
        ratiotemp.resize(3);
        float temp1 = 0.0;
        float temp2 = 0.0;

        temp1 = distanceOfPoint(point1onA, point2onA);
        temp2 = distanceOfPoint(point1onB, point2onB);
        ratiotemp[0] = temp1/temp2;

        temp1 = distanceOfPoint(point2onA, point3onA);
        temp2 = distanceOfPoint(point2onB, point3onB);
        ratiotemp[1] = temp1/temp2;

        temp1 = distanceOfPoint(point1onA, point3onA);
        temp2 = distanceOfPoint(point1onB, point3onB);
        ratiotemp[2] = temp1/temp2;

        ratioOut = (ratiotemp[0]+ratiotemp[1]+ratiotemp[2])/3;
        return 1;
}

int CalculateThreePointMatrixTransform::caliRotation(vector<float> point1onA,vector<float> point2onA,vector<float> point3onA,
                     vector<float> point1onB,vector<float> point2onB,vector<float> point3onB,
                                                     float ratioIn,Eigen::Matrix3f& rotationOut)
{
      //2点减1点
      float m12 = (point2onA[2] - point1onA[2]) + ratioIn*(point2onB[2] - point1onB[2]);

      float m32 = (point2onA[0] - point1onA[0]) + ratioIn*(point2onB[0] - point1onB[0]);

      float m31 = (point2onA[1] - point1onA[1]) + ratioIn*(point2onB[1] - point1onB[1]);

      float m21 = m12;

      //3点减1点
      float m23 = (point3onA[0] - point1onA[0]) + ratioIn*(point3onB[0] - point1onB[0]);

      float m13 = (point3onA[1] - point1onA[1]) + ratioIn*(point3onB[1] - point1onB[1]);


      Eigen::Matrix3f J ;
      J<<0,-m21,-m31,   -m12,0,m32,    m13,m23,0;

      //let J = matrix_float3x3(float3(0,-m12,m13),float3(-m21,0,m23),float3(-m31,m32,0))

      Eigen::FullPivLU< Eigen::Matrix3f > tmpMaLu(J);
      if(false==tmpMaLu.isInvertible())
      {
          //3个点的x,y,z都需要有变化，否则为０，可能导致矩阵不可逆，无法求解．
          return -1;
      }


      Eigen::Matrix3f  K = J.inverse();//bug ,好像无逆矩阵


//      var xyz = float3(0,0,0);
      Eigen::Vector3f xyz;

      xyz(0) = -1 * ratioIn*(point2onA[0] - point1onA[0]) + (point2onB[0] - point1onB[0]);

      xyz(1) = -1 * ratioIn*(point2onA[1] - point1onA[1]) + (point2onB[1] - point1onB[1]);

      xyz(2) = -1 * ratioIn*(point3onA[2] - point1onA[2]) + (point3onB[2] - point1onB[2]);

      Eigen::Vector3f abc;//向量
      abc= K * xyz;


      Eigen::Matrix3f S;
//      S<<0, abc[2], abc[1], -1*abc[2], 0, abc[0], -1*abc[1], -1*abc[0], 0;
      S<<0,-1*abc[2],-1*abc[1],   abc[2],0,-1*abc[0],   abc[1],  abc[0],0;
      // let S = matrix_float3x3(float3(0, abc.z, abc.y), float3(-1*abc.z, 0, abc.x), float3(-1*abc.y, -1*abc.x, 0))

      Eigen::Matrix3f I = Eigen::Matrix3f::Identity();


      rotationOut = (I + S) * ((I - S).inverse()) * ratioIn;
      return 1;
}

int CalculateThreePointMatrixTransform::  caliTranslation(vector<float> point1onA,vector<float> point1onB,
                                   Eigen::Matrix3f rotation, Eigen::Vector3f& translationOut)
{
    Eigen::Vector3f pointonA,pointonB;
    pointonA<<point1onA[0],point1onA[1],point1onA[2];
    pointonB<<point1onB[0],point1onB[1],point1onB[2];
    translationOut = pointonA - rotation * pointonB;
    return 1;
}

float CalculateThreePointMatrixTransform::distanceOfPoint(vector<float> point1, vector<float> point2)
{
    return sqrt((point1[0]-point2[0])*(point1[0]-point2[0])+
                (point1[1]-point2[1])*(point1[1]-point2[1])+(point1[2]-point2[2])*(point1[2]-point2[2]));
}



//输入一维数组/向量matrixa（X,Y,Z）和matrixb(X,Y,Z)，长度均为3；
//输出一维数组/向量result(X,Y,Z)，为垂直于该两向量的向量，长度为3
double CalculateThreePointMatrixTransform:: CrossMultiply(double *matrixa, double  *matrixb, double *VectorResult)
{
    double temp[3];
    temp[0] = matrixa[1] * matrixb[2] - matrixa[2] * matrixb[1];
    temp[1] = matrixa[2] * matrixb[0] - matrixa[0] * matrixb[2];
    temp[2] = matrixa[0] * matrixb[1] - matrixa[1] * matrixb[0];
    VectorResult[0] = temp[0] / sqrt(temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2]);
    VectorResult[1] = temp[1] / sqrt(temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2]);
    VectorResult[2] = temp[2] / sqrt(temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2]);
/*	VectorResult[0] = matrixa[1] * matrixb[2] - matrixa[2] * matrixb[1];
    VectorResult[1] = matrixa[2] * matrixb[0] - matrixa[0] * matrixb[2];
    VectorResult[2] = matrixa[0] * matrixb[1] - matrixa[1] * matrixb[0]*/;
    return VectorResult[0];
}

double CalculateThreePointMatrixTransform:: Rref(double *RotateB, double  *RotateA, double *RotateBToA)
{
    //int ACount = end(*RotateA) - begin(*RotateA);
    //int ACount = sizeof(*RotateA) + 1;
    int ACount = 9;
    //int ACount = sizeof(*RotateA) / sizeof(RotateA[0]);
    //int  BCount = sizeof(*RotateB) + 1;
    int  BCount = 9;
    double  ManageObject[1000];
    int  ARowNum = sqrt(ACount);
    int BColNum = BCount / ARowNum;

    double  Diagonal1;
    double  Diagonal2;

    if (ARowNum*ARowNum == ACount)
    {

        for (int i = 0; i< ARowNum; i++)
        {
            for (int j = 0; j<(ARowNum + BColNum); j++)
            if (j > ARowNum - 1)
            {
                ManageObject[i*(ARowNum + BColNum) + j] = RotateB[i*(BColNum)+j - ARowNum];
            }
            else
            {
                ManageObject[i*(ARowNum + BColNum) + j] = RotateA[i*(ARowNum)+j];
            }
        }

        //进行化简
        for (int i = 0; i < ARowNum; i++)
        {
            if (ManageObject[i*(ARowNum + BColNum) + i] != 0)

            {
                //处理当前行
                Diagonal1 = ManageObject[i*(ARowNum + BColNum) + i];
                for (int j = 0; j < (ARowNum + BColNum); j++)
                {
                    ManageObject[i*(ARowNum + BColNum) + j] = ManageObject[i*(ARowNum + BColNum) + j] / Diagonal1;
                }

                //处理其他行
                for (int mm = 0; mm < ARowNum; mm++)
                {
                    Diagonal2 = (ManageObject[mm*(ARowNum + BColNum) + i] / ManageObject[i*(ARowNum + BColNum) + i]);
                    for (int nn = 0; nn < (ARowNum + BColNum); nn++)
                    {
                        if (i != mm)
                        {
                            ManageObject[mm*(ARowNum + BColNum) + nn] = ManageObject[mm*(ARowNum + BColNum) + nn] - Diagonal2*ManageObject[i*(ARowNum + BColNum) + nn];
                        }
                    }

                }
            }

        }
    }
    for (int i = 0; i < ARowNum; i++)
    {
        for (int j = 0; j < BColNum; j++)
        {
            RotateBToA[j*BColNum + i] = ManageObject[i* (ARowNum + BColNum) + (ARowNum + j)];
        }
    }
    //return RotateBToA[0];
    return RotateBToA[0];
}



//输入A坐标系下三个点的坐标Point1InA，Point2InA，Point3InA，B坐标系下的三个点坐标Point1InB，Point2InB，Point3InB
//输出A到B的旋转矩阵dOutRotateMatrixAToB，A到B的平移矩阵dOutTranslationMatrixAToB
//输入坐标都是右手系坐标
double CalculateThreePointMatrixTransform:: ThreePointsTransMatrix(double *Point1InA, double *Point2InA,
                                                                   double *Point3InA,
                                        double *Point1InB, double *Point2InB, double *Point3InB,
                                        double *dOutRotateMatrixAToB, double *dOutTranslationMatrixAToB,
                                        double *Deviation)
{
    double  Vector12OfA[3];
    double  Vector13OfA[3];
    double  Vector32OfA[3];
    Vector12OfA[0] = Point1InA[0] - Point2InA[0];
    Vector12OfA[1] = Point1InA[1] - Point2InA[1];
    Vector12OfA[2] = Point1InA[2] - Point2InA[2];
    Vector13OfA[0] = Point1InA[0] - Point3InA[0];
    Vector13OfA[1] = Point1InA[1] - Point3InA[1];
    Vector13OfA[2] = Point1InA[2] - Point3InA[2];

    CrossMultiply(Vector12OfA, Vector13OfA, Vector32OfA);
     CrossMultiply(Vector32OfA, Vector12OfA, Vector13OfA);

     double Vector12OfB[3];
     double Vector13OfB[3];
     double Vector32OfB[3];
     Vector12OfB[0] = Point1InB[0] - Point2InB[0];
     Vector12OfB[1] = Point1InB[1] - Point2InB[1];
     Vector12OfB[2] = Point1InB[2] - Point2InB[2];
     Vector13OfB[0] = Point1InB[0] - Point3InB[0];
     Vector13OfB[1] = Point1InB[1] - Point3InB[1];
     Vector13OfB[2] = Point1InB[2] - Point3InB[2];

     CrossMultiply(Vector12OfB, Vector13OfB, Vector32OfB);
     CrossMultiply(Vector32OfB, Vector12OfB, Vector13OfB);


     double MatrixB[9];
     MatrixB[0] = Vector12OfB[0];
     MatrixB[1] = Vector12OfB[1];
     MatrixB[2] = Vector12OfB[2];
     MatrixB[3] = Vector13OfB[0];
     MatrixB[4] = Vector13OfB[1];
     MatrixB[5] = Vector13OfB[2];
     MatrixB[6] = Vector32OfB[0];
     MatrixB[7] = Vector32OfB[1];
     MatrixB[8] = Vector32OfB[2];

     double  MatrixA[9];
     MatrixA[0] = Vector12OfA[0];
     MatrixA[1] = Vector12OfA[1];
     MatrixA[2] = Vector12OfA[2];
     MatrixA[3] = Vector13OfA[0];
     MatrixA[4] = Vector13OfA[1];
     MatrixA[5] = Vector13OfA[2];
     MatrixA[6] = Vector32OfA[0];
     MatrixA[7] = Vector32OfA[1];
     MatrixA[8] = Vector32OfA[2];
     double RotateAToB[18];
     Rref(MatrixA, MatrixB, RotateAToB);

     dOutRotateMatrixAToB[0] = RotateAToB[0];
     dOutRotateMatrixAToB[1] = RotateAToB[1];
     dOutRotateMatrixAToB[2] = RotateAToB[2];
     dOutRotateMatrixAToB[3] = RotateAToB[3];
     dOutRotateMatrixAToB[4] = RotateAToB[4];
     dOutRotateMatrixAToB[5] = RotateAToB[5];
     dOutRotateMatrixAToB[6] = RotateAToB[6];
     dOutRotateMatrixAToB[7] = RotateAToB[7];
     dOutRotateMatrixAToB[8] = RotateAToB[8];

     dOutTranslationMatrixAToB[0] = Point1InA[0] - (Point1InB[0] * dOutRotateMatrixAToB[0] + Point1InB[1] * dOutRotateMatrixAToB[1] + Point1InB[2] * dOutRotateMatrixAToB[2]);
     dOutTranslationMatrixAToB[1] = Point1InA[1] - (Point1InB[0] * dOutRotateMatrixAToB[3] + Point1InB[1] * dOutRotateMatrixAToB[4] + Point1InB[2] * dOutRotateMatrixAToB[5]);
     dOutTranslationMatrixAToB[2] = Point1InA[2] - (Point1InB[0] * dOutRotateMatrixAToB[6] + Point1InB[1] * dOutRotateMatrixAToB[7] + Point1InB[2] * dOutRotateMatrixAToB[8]);

     Eigen::Vector3d dev1, dev2,dev3, V3Temp, V3Temp1,Trans;
     Eigen::Matrix3d MyRotate;
     MyRotate << dOutRotateMatrixAToB[0], dOutRotateMatrixAToB[1], dOutRotateMatrixAToB[2],
         dOutRotateMatrixAToB[3], dOutRotateMatrixAToB[4], dOutRotateMatrixAToB[5],
         dOutRotateMatrixAToB[6], dOutRotateMatrixAToB[7], dOutRotateMatrixAToB[8];
     Trans << dOutTranslationMatrixAToB[0],
         dOutTranslationMatrixAToB[1],
         dOutTranslationMatrixAToB[2];

     V3Temp << Point1InA[0], Point1InA[1], Point1InA[2];
     V3Temp1 << Point1InB[0], Point1InB[1], Point1InB[2];
     dev1 = V3Temp - MyRotate*V3Temp1 - Trans;

     V3Temp << Point2InA[0], Point2InA[1], Point2InA[2];
     V3Temp1 << Point2InB[0], Point2InB[1], Point2InB[2];
     dev2 = V3Temp - MyRotate*V3Temp1 - Trans;

     V3Temp << Point3InA[0], Point3InA[1], Point3InA[2];
     V3Temp1 << Point3InB[0], Point3InB[1], Point3InB[2];
     dev3 = V3Temp - MyRotate*V3Temp1 - Trans;
     Deviation[0] = dev1(0);
     Deviation[1] = dev1(1);
     Deviation[2] = dev1(2);
     Deviation[3] = dev2(0);
     Deviation[4] = dev2(1);
     Deviation[5] = dev2(2);
     Deviation[6] = dev3(0);
     Deviation[7] = dev3(1);
     Deviation[8] = dev3(2);
     return dOutRotateMatrixAToB[0];
}


